#include "perception_visualizer_debug.h"

#include "common/utility.h"

namespace autodrive {
namespace perception {
PerceptionVisualizerDebug::PerceptionVisualizerDebug()
    : is_init_{false}, is_dispose_{false}, is_update_vis_{false} {}

PerceptionVisualizerDebug::~PerceptionVisualizerDebug() {
  is_dispose_ = true;
  if (vis_thread_.joinable()) {
    vis_thread_.join();
  }
}

PerceptionVisualizerDebug& PerceptionVisualizerDebug::GetInstance() noexcept {
  static PerceptionVisualizerDebug instance;
  return instance;
}

void PerceptionVisualizerDebug::UpdateMatch(
    MapElementPointSet const& predictPoints,
    MapElementPointSet const& currentPoints,
    std::string const& matchInfo) noexcept {
  {
    std::lock_guard<std::mutex> lock(update_vis_lock_);
    match_cloud_ptr_->clear();
    for (auto const& ep : predictPoints) {
      pcl::PointXYZRGB p;
      p.x = ep.x;
      p.y = ep.y;
      p.z = ep.z;
      p.r = 255;
      p.g = 0;
      p.b = 0;
      match_cloud_ptr_->emplace_back(p);
    }

    for (auto const& ep : currentPoints) {
      pcl::PointXYZRGB p;
      p.x = ep.x;
      p.y = ep.y;
      p.z = ep.z;
      p.r = 0;
      p.g = 255;
      p.b = 0;
      match_cloud_ptr_->emplace_back(p);
    }
    match_info_ = matchInfo;
    is_update_vis_ = true;
    is_continue_ = false;
  }

  while (true) {
    if (is_continue_ || is_dispose_) {
      break;
    }
    msleep(100);
  }
}

void PerceptionVisualizerDebug::Init() noexcept(false) {
  is_dispose_ = false;
  is_update_vis_ = false;
  auto initFun{[&]() {
    if (cloud_ptr_ == nullptr) {
      cloud_ptr_ = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    }

    if (match_cloud_ptr_ == nullptr) {
      match_cloud_ptr_ = pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    }

    vis_thread_ = std::thread{vis_task, this};
  }};
  std::call_once(init_flag_, initFun);
  is_init_ = true;
}

void PerceptionVisualizerDebug::vis_task(
    PerceptionVisualizerDebug* const visPtr) noexcept {
  try {
    auto pclVisPtr{pcl::visualization::PCLVisualizer::Ptr(
        new pcl::visualization::PCLVisualizer("Map Viewer"))};
    if (pclVisPtr == nullptr) {
      throw std::runtime_error("pcl visualizer create fail");
    }
    pclVisPtr->initCameraParameters();
    pclVisPtr->setCameraPosition(0, 0, 120, 1, 0, 0);
    pclVisPtr->setShowFPS(true);

    pclVisPtr->registerKeyboardCallback(
        [&](const pcl::visualization::KeyboardEvent& kEven) {
          if (kEven.isShiftPressed()) {
            std::lock_guard<std::mutex> lock(visPtr->update_vis_lock_);
            visPtr->is_continue_ = true;
            std::cout << "Continue..." << std::endl;
          }
        });

    int viewPortResult{0};
    pclVisPtr->createViewPort(0.0, 0.0, 0.5, 1.0, viewPortResult);
    pclVisPtr->setBackgroundColor(0.15, 0.15, 0.15, viewPortResult);
    std::string const& resultTextID{"result text"};
    pclVisPtr->addText("Result", 10, 10, 25, 1, 1, 1, resultTextID,
                       viewPortResult);

    int viewPortMatch(0);
    pclVisPtr->createViewPort(0.5, 0.0, 1.0, 1.0, viewPortMatch);
    pclVisPtr->setBackgroundColor(0.2, 0.2, 0.2, viewPortMatch);
    pclVisPtr->addText("Match", 10, 10, 25, 1, 1, 1, "Match text ",
                       viewPortMatch);

    pclVisPtr->addCoordinateSystem(1.0);

    if (visPtr->cloud_ptr_ == nullptr) {
      visPtr->cloud_ptr_ =
          pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
    }

    std::string const& resultCloudID{"ResultCloud"};
    std::string const& matchCloudID{"MatchCloud"};

    auto displayCloud{[&pclVisPtr](
                          pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPtr,
                          std::string const& id, int viewPort = 0) {
      if (cloudPtr == nullptr) {
        return;
      }
      pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(
          cloudPtr);

      pclVisPtr->addPointCloud<pcl::PointXYZRGB>(cloudPtr, rgb, id, viewPort);

      pclVisPtr->setPointCloudRenderingProperties(
          pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, id, viewPort);
    }};

    std::string const& timestampID{"timestampID"};
    pclVisPtr->addText("timestamp", 100, 10, 25, 1, 1, 0, timestampID,
                       viewPortResult);

    std::string const& matchInfoID{"matchInfoID"};
    pclVisPtr->addText("match info", 100, 100, 25, 1, 1, 0, matchInfoID,
                       viewPortMatch);

    while (true) {
      if (visPtr->is_dispose_) {
        return;
      }

      {
        std::lock_guard<std::mutex> lock(visPtr->update_vis_lock_);
        if (visPtr->is_update_vis_) {
          pclVisPtr->removeAllPointClouds();

          displayCloud(visPtr->cloud_ptr_, resultCloudID, viewPortResult);

          displayCloud(visPtr->match_cloud_ptr_, matchCloudID, viewPortMatch);

          pclVisPtr->updateText(std::to_string(visPtr->timestamp_), 100, 10,
                                timestampID);
          pclVisPtr->updateText(visPtr->match_info_, 100, 100, matchInfoID);

          visPtr->is_update_vis_ = false;
        }
      }

      if (!pclVisPtr->wasStopped()) {
        pclVisPtr->spinOnce(100);
        // sleep(10);
      } else {
        visPtr->is_dispose_ = true;
      }
    }
  } catch (const std::exception& e) {
    visPtr->is_dispose_ = true;
    std::cout << "[" << to_red("Error") << "]" << "OccupancyVisualizer error, "
              << e.what() << std::endl;
  } catch (...) {
    visPtr->is_dispose_ = true;
    std::cout << "[" << to_red("Error") << "]"
              << "OccupancyVisualizer unexpected error!!!" << std::endl;
  }
}

}  // namespace perception
}  // namespace autodrive